function [acc,lambda,epsilonT,state] = solver(state_past,method,XT,YT)
X = state_past(1);
Y = state_past(2);
theta = state_past(3);
Xgo = XT - X;
Ygo = YT - Y;
Rgo = sqrt(Xgo^2+Ygo^2);
lambda = atan(Ygo/Xgo);
h = 0.1;
N = 3;
Td = 40;
V = 300;
% dlambda = (lambda - lambda_past)/h;
dlambda = -V*sin(theta - lambda)/Rgo;
ap = N*V*dlambda;
Tgo = Rgo*(1 + (theta - lambda)^2/10)/V;
epsilonT = Td - Tgo;
switch method
    case{'PNG'}
        acc = ap;
    case{'ITCG'}
        % 式(24)
        Ke = -60*(V^5)/ap/(Rgo^3);
        acc = ap + Ke*epsilonT; 
        % 式(20)
%         Ke = (240*V^5)/(ap^2)/(Rgo^3);
%         acc = ap*(1.5-0.5*sqrt(1 + Ke*epsilonT));
end
    k1 = h * dery(state_past, acc);
    k2 = h * dery(state_past + 0.5 * k1, acc);
    k3 = h * dery(state_past + 0.5 * k2, acc);
    k4 = h * dery(state_past + k3, acc);
    state = state_past + (k1 + 2 * k2 + 2 * k3 + k4) / 6;
end

function dstate = dery(state,acc)
% 状态量  X,Y,theta
V = 300;
theta = state(3);
sizes = size(state);
dstate = zeros(sizes(1),1);
dstate(1) = V*cos(theta);
dstate(2) = V*sin(theta);
dstate(3) = acc/V;
end